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Abstract :  The  paper  discusses  the  use  of  dual 
stave  forward-looking  sonar  to  build  a  three- 
dimensional  underwater  model  for  navigation 
in  restricted  waterways.  The  sonar  normally 
provides  separate  horizontal  and  vertical 
images  but  when  the  information  is 
synthesized  into  a  single  three-dimensional 
model  it  permits,  in  certain  circumstances,  the 
AUV  to  plan  and  carry  out  a  trajectory  where 
no  path  is  otherwise  available.  As  the  sonar 
system  is  fixed  mounted  to  the  AUV  body,  a 
trajectory  is  presented  which  uses  the 
combined  sonar  fields-of-view  to  produce 
paths  that  optimize  situational  understanding 
for  reactive  and  deliberative  obstacle 
avoidance. 

1.0  INTRODUCTION 

Currently  Autonomous  Underwater  Vehicles 
(AUV)  are  used  primarily  in  open  oceans  where 
the  primary  danger  to  the  vehicles  are  from 
unexpected  navigation  hazards  on  the  ocean 
floor  and  surface.  More  and  more,  vehicles  are 
being  tasked  with  collecting  information  in  more 
congested,  restrictive  waterways  such  as  harbors 
and  rivers.  Due  to  the  dynamism  of  the 
environment,  it  requires  an  AUV  to  have  more 
flexibility  in  path  planning  and  reactive 
avoidance  behaviors.  This  requirement  for 
greater  perceptual  awareness  is  made  possible 
through  deliberate  navigation  strategies  for  map 
building. 


Forward  Looking  Sonar  has  developed  to  the 
point  that  small  AUVs  can  use  it  for  obstacle 
detection,  avoidance  [1,2]  and  map  building.  The 
configuration  of  the  sonar  produces  separate 
horizontal  and  vertical  images. 

Two  examples  of  sonar  images  are  given  in 
Figures  one  and  two.  These  images  were  taken  as 
the  AUV  approached  a  bridge.  Notice  that  in  the 
horizontal  image  there  is  a  strong  intensity 
across  the  entire  field  of  view  at  approximately 
30  meters  distance  from  the  vehicle.  This  is  the 
underwater  ground  buildup  to  support  the  bridge 
pylons.  At  about  45  meters  from  the  vehicle  are 
the  bridge  pylons,  these  appear  as  vertical  lines 
in  the  image. 
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Figure  1.  Horizontal  Sonar  Image  -  AUV 
approaches  bridge 
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Figure  two  is  a  vertical  sonar  image  taken  at 
approximately  the  same  time.  It  includes 
annotations  for  the  riverbed,  river  surface  and 
pylon(s)  obstructing  the  vehicle’s  path  forward. 
In  other  words,  automated  image  processing  and 
path  planning  algorithms  wouldn’t  be  able  to 
produce  a  trajectory  underneath  the  bridge  using 
either  of  the  images.  It  will  be  shown  that  only 
by  combining  horizontal  and  vertical  sonar 
models  together  with  the  intensity  images  over 
time,  is  it  possible  to  develop  a  viable,  safe  path 
underneath  the  bridge. 


Figure  2.  Vertical  Sonar  Image  -  AUV 
approaches  bridge 

Since  the  sonar  system  is  fixed  to  the  nose  of  the 
AUV  body,  the  only  way  to  slew  the  horizontal 
and  vertical  sonar  staves  is  through  the  vehicle 
actuation.  Non-linear  trajectories  can  be 
designed  to  maximize  the  situational 
understanding  for  the  AUV.  This  is  necessary  to 
more  fully  understand  the  environment  forward 
of  the  AUV  for  making  time-critical  decisions 
associated  with  collision-free  paths.  These 
trajectories  must  be  developed  in  real-time  and 
take  into  consideration  the  vehicle  dynamics, 
sensory  information  and  situational 
understanding  represented  by  its  internal  map. 

The  paper  discusses  the  following:  First  it 
describes  the  AUV  and  sonar  used  in  the 
developmental  testing.  Second,  it  develops  a 
three-dimensional  model  of  the  horizontal  and 
vertical  sonar  staves.  Third,  it  discusses  the  use 
of  probabilistic  occupancy  grids  to  build  an 
internal  representation  of  the  environment. 
Fourth,  it  discusses  the  image  processing 
techniques  necessary  to  deal  with  noise  sources 
that  inhibit  accurate  map  building.  Fifth,  it 
discusses  results  based  on  data  sets  collected  in 
the  Charles  River,  Cambridge  MA  with  an  AUV 


navigating  in  and  around  the  Massachusetts 
Avenue  Harvard  Bridge.  Sixth,  it  discusses  a 
control  law  for  maximizing  situational  awareness 
in  restricted  waterway  areas. 

2.0  SYSTEM  DESCRIPTION 

The  system  used  for  the  research  is  a  modified 
REMUS  100  AUV  with  the  Blue  view  Forward 
Looking  Sonar  and  is  listed  in  Figure  3.  The 
sonar  module  contains  a  secondary  or  “backseat” 
CPU  controller  that  permits  real-time  sonar 
image  processing  and  path  planning.  The  AUV 
has  a  communication  protocol  that  permits  the 
backseat  controller  to  take  overriding  control; 
this  functionality  is  very  useful  for  prototyping 
autonomous  behaviors. 


Figure  3.  NPS  REMUS  AUV 
2.1  AUV  DESCRIPTION 


The  Charles  River  data  set  was  collected  on  a 
REMUS  specially  configured  with  an  Integrated 
Navigation  System  (INS)  that  provided  accurate 
position  estimation  to  within  .5  percent  distance 
traveled  by  combining  together  accurate 
accelerometers  and  rate  gyros  with  velocity 
estimates  from  the  Doppler  Velocity  Log  (DVL) 
and  position  fixes  from  a  GPS1.  The  starting 
point  for  the  missions  was  the  MIT  Sailing 
Pavilion  dock. 

2.2  SONAR  DESCRIPTION 

The  Blueview  P450-15E  Forward  Looking  Sonar 
was  mounted  to  the  front  of  the  AUV.  The  sonar 
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has  four  horizontally  mounted  staves  that 
produce  a  single  image  with  a  ninety  degree  field 
of  view.  The  sonar  also  has  two  vertically 
mounted  staves  that  produce  a  forty- six  degree 
field  of  view  and  result  in  a  single,  vertical 
image.  In  each  case  the  range  resolution  is 
approximately  2  inches  and  the  bearing 
resolution  is  approximately  1  degree. 

3.0  SONAR  MODELING 

The  image  created  by  the  sonar  is  a  two- 
dimensional  representation  of  a  three- 
dimensional  volume.  A  depiction  of  a  nominal 
volume  ensonified  by  the  horizontally  sonar  is 
depicted  in  Figure  5. 


Figure  5.  Horizontal  Sonar  Model 


A  single  stave  of  the  P450-15E  has  an 
approximate  23 -degree  field  of  view  in  the  image 
plane.  The  ensonified  volume  covers  a  fifteen- 
degree  spread  in  the  vertical  plane.  The  sonar  is 
unable  to  distinguish  differences  of  an  object  at 
different  elevations  within  this  region.  For 
example,  the  object  highlighted  in  Figure  6, 
could  be  located  anywhere  within  the  fifteen- 
degree  spread.  Based  on  the  corresponding 
distance  from  the  AUV  this  is  a  spread  of  +-  4 
meters.  This  is  the  ambiguity  of  the  sonar  in  the 
vertical  plane. 


Figure  6.  Horizontal  Image  with  circled  object 
30  meters  from  the  AUV. 


The  vertical  array  is  composed  of  a  two  staves 
mounted  perpendicular  to  the  horizontal  array. 
Mounting  the  array  in  this  fashion  provides 
angular  resolution  in  the  vertical  direction.  Due 
to  the  rotation  of  the  array,  the  ambiguity  of  the 
sonar  shifts  to  the  horizontal  direction.  All 
vertical  sonar  images  will  have  fifteen  degrees  of 
ambiguity  in  the  horizontal  direction.  The 
vertical  sonar  is  used  to  provide  the  location  of 
the  ocean  floor  and  the  height  of  the  objects 
encountered  proud  of  the  ocean  floor.  The 
vertical  sonar  head  is  rigidly  mounted  on  the 
AUV  with  an  approximate  5  degree  downward 
angle. 

By  combining  the  separate  fields  of  view,  the 
two  sonars  can  effectively  create  a  volume  with 
resolution  in  all  three  planes.  The  volume 
ensonified  by  the  combination  of  sonars  is 
depicted  in  Figure  7. 


Figure  7.  A  combined  sonar  model  depicting 
the  vertical  and  horizontal  forward  looking 
sonar 

Of  particular  relevance  is  the  fifteen  by  fifteen- 
degree  area  where  the  two  arrays  overlap.  By 
combining  the  resolution  of  each  sonar,  it 
effectively  creates  a  cross  section  where  the 
resolution  is  two  inches  in  range  and  one  degree 
in  vertical  width  and  one  degree  in  horizontal 
width. 

3.1  GEO-LOCATING  IMAGE  DATA 

The  sonar  system  is  fixed  mounted  to  the  front  of 
the  vehicle.  The  FLS  data  is  referenced  to  the 
vehicle’s  position;  it  has  a  local  reference  frame. 
To  be  useful,  the  data  needs  to  be  transformed 
into  the  global  space.  By  combining  together  the 
vehicle  position  with  the  FLS  model  each  data 
point  can  be  geo-located.  This  results  in  the 
following  equations  for  translating  a  locally 
referenced  sonar  image  pixel  into  a  global 
reference  frame. 


XP  =  xv  +  rp  cos<+  +  ep )  cos(</»  +  <pp ) 

yp  =  yv  +  rp  sin(0v  +  ep )  cos(<ft  +  ) 
Zp=zv  +  rpsin((pv+(pp) 


Where  (x  ,  yp ,  Zp  )  is  the  globally  referenced 

location  of  the  pixel  The  above  equations  will 
map  data  stored  in  the  image  space  into  the 
global  space. 

The  next  step  is  to  separate  detections  from 
image  noise.  This  will  be  done  through  the  use  of 
occupancy  grids  where  the  elements  in  each  grid 
will  be  converted  from  an  intensity  pixel  to  a 
probability  of  an  object  being  located  in  the  grid 
cell. 

4.0  OCCUPANCY  GRIDS 

To  permit  deliberative  and  reactive  path  planning 
a  robot  needs  an  internal  representation  of  it’s 
external  environment  -  in  other  words  it  needs  a 
map.  To  create  a  safe  path  the  vehicle  needs  to 
know  the  location  of  obstacles  and  the  location 
of  free  space.  An  additional  requirement  for  the 
vehicle  is  to  create  obstacle-free  trajectories  that 
are  globally  consistent  and  not  subject  to 
trapping  conditions  -  normally  described  as  local 
minima.  Probabilistic  occupancy  grids  are  an 
appropriate  and  accepted  methodology  for 
building  and  maintaining  an  internal  map  [4].  In 
this  case  the  goal  is  to  build  and  maintain  a 
three-dimensional  occupancy  grid  that  is  used  for 
real-time  path  planning. 

Occupancy  grids  divide  the  environment  into 
gridded  cells.  Each  cell  is  evaluated  to  determine 
whether  or  not  an  object  is  present.  The  range  of 
values  of  each  cell  is  between  0  (no  object 
present)  and  1  (object  present).  The  cells  are 
initialized  to  .5,  which  represents  impartiality  of 
an  obstacle  presence.  Because  it  is  not  necessary 
to  recognize  and  organize  objects  in  the 
underwater  map  by  their  shape  and  location, 
occupancy  grids  is  an  appropriate  methodology 
for  avoidance  maneuvers. 

Bayes  Theorem  is  used  to  determine  the 
probability  of  the  state  of  the  cell  given  the 
current  reading  from  the  sonar.  Based  on  this, 
Elfes  provides  an  iterative  solution  to 
determining  the  probability  that  a  cell  is 
occupied  given  all  previous  measurements. 
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r  :  The  current  measurement 
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s(c):  State  of  the  cell 

jr}  :  All  measurement  up  to  time  t 


The  definitions  of  the  terms  are  as  follows: 
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Prob  a  cell  is  occupied 
given  the  current  and  all 
prior  measurements 

p[ 
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Prob  a  cell  is  occupied 
given  all  the  prior 
measurements 

’(c)  =  Empty  1  {r}^ 

Prob  a  cell  is  empty 
given  all  the  prior 
measurements 

P 

[+ls(C)  =  0cc] 

Prob  of  receiving  the 
current  measurement 
given  the  cell  is 
occupied 

r+l  1  s(c)  =  Empty 

\ 

Prob  of  receiving  the 
current  measurement 
given  the  cell  is  empty 

Table  1.  Definition  of  terms 


The  summation  in  the  denominator  represents 
two  cases  -  either  the  cell  is  occupied  or  empty. 
Since  the  cells  can  have  only  two  states 

p[s(c)  =  Empty  I  {r}  ]  can  be  replaced  by 

1-P[s(c)  =  0ccl{r}l  .  As  an  example,  assume 
that  an  occupancy  grid  cell  has  a  prior 
probability  P  [s(c)  =  Occ  I  {r}  ]  =  .6 ,  and  a 

new  measurement  arrives.  Based  on  the  new 
measurement  and  the  corresponding  probability 

distributions  the  values  of  p[r  \  s(c)  =  Occ\ 

and  p\r  I  s(c)  =  Empty]  are  determined. 

Assume  those  are  respectively  equal  to  .2  and  .4. 
From  these  values  the  probability  of  the  cell 
being  occupied  is  updated. 

P[s(c)  =  0ccl{r}t  ]  = 

0.4  *0.6 

- =  .75 

0.2  *(1-. 6) +  0.4  *0.6 


As  the  AUV  navigates,  an  update  occurs  for  each 
globally  mapped  occupancy  grid  cell.  In  other 
words,  as  the  sonar  sensor  information  becomes 
available  the  range  of  updated  cells  reflects  the 
transformed  local  sensor  information  into  the 
global  map.  The  next  step  is  to  determine  the  two 
probability  distributions  given  the  cell  is  either 
empty  or  occupied. 

4.1  FORWARD  LOOKING  SONAR 
PROBABILITY  MODELS 

To  populate  an  occupancy  grid,  requires  the 
development  of  probability  models  for  the  FLS. 
For  a  sensor  to  be  used  in  an  occupancy  grid, 
two  separate  probabilistic  models  need  to  be 
generated.  One  model  describes  the  probability 
of  receiving  a  value  when  the  return  can  be 
attributed  solely  to  noise.  The  second  model 
describes  the  probability  of  receiving  a  particular 
value  when  the  sonar  return  can  be  attributed  to  a 
physical  object. 

The  development  of  the  models  is  made  more 
difficult  from  the  sporadic  noise  generated  by  the 
forward-looking  sonar.  Noise  is  generated  by 
both  internal  and  external  sources.  The  noise 
sources  internal  to  the  vehicle  include  power 
supply  fluctuations,  Acoustic  Doppler  Current 
Profiler  (ADCP)  and  the  Acoustic  Modem. 
Noise  sources  external  to  the  vehicle  includes 
weather,  biological  sources  and  other  vehicles  or 
man-made  sources.  To  begin  we  will  discuss  the 
background  FLS  noise  model.  This  methodology 
is  applicable  for  both  the  horizontal  and  vertical 
images. 

4.1.1  BACKGROUND  NOISE  MODEL 

The  blazed  array  sonar  system  utilizes  a  principle 
similar  to  echelette  diffraction  gratings  in  optics 
to  create  a  multitude  of  beams  from  a  single 
sound  source  [6].  The  blazed  sonar  array  maps 
the  frequency  of  a  given  broadband  pulse  to  the 
angular  spatial  domain.  This  creates  an  image 
that  contains  a  low  intensity  random  signal  that 
varies  with  bearing  angle.  Because  it  varies,  a 
uniform  threshold  filter  is  insufficient  for 
describing  the  background  noise. 

To  adequately  handle  this  image  diversity,  it  is 
necessary  to  develop  a  per  pixel  noise  model.  To 
do  so  we  took  a  data  set  of  300  sonar  images  that 
contained  few  objects  in  the  field  of  view. 
Because  of  the  multitude  of  noise  sources  a 
histogram  was  used  to  observe  the  distribution. 


The  histogram  contained  25  bins  with  an 
approximate  spacing  of  the  image  intensity 
values  of  160.  Figures  9a,  9b  and  9c  give 
examples  of  histogram  distributions. 
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Figure  9a.  Noise  Histogram  for  the  horizontal 
FLS  at  Range  88m  and  -45  degrees  bearing 
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Figure  9b.  Noise  Histogram  for  the  horizontal 
FLS  at  Range  88m  and  -5  degrees  bearing 
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Figure  9c.  Noise  Histogram  for  the  horizontal 
FLS  at  Range  88m  and  25  degrees  bearing 

4.1.2  OBJECT  DETECTION  MODEL 

The  process  to  develop  the  object  detection 
model  was  similar  to  the  background  noise 
model.  Since  the  object  detections  were  not  as 
common  on  a  per  pixel  basis  and  the  intensity 
from  an  object  in  the  sonar  return  was  not  as 
prone  to  intensity  fluctuations  over  bearing  and 
range,  a  single  global  model  was  developed.  To 
create  the  model  200  images  in  each  plane  were 
annotated  using  the  LabelMe  annotation  tool. 
Then  the  areas  containing  only  objects  were 


extracted  and  a  25  bin  histogram  was  created  of 
the  intensities  for  each  plane.  Figure  10  gives  a 
plot  of  the  pixel  intensities  from  the  objects 
selected  from  the  horizontal  images.  This 
normalized  histogram  can  be  then  used  as  the 
detection  probability  distribution. 


Figure  10.  Normalized  histogram  of  pixel 
intensities  from  selected  objects  in  horizontal 
sonar  images.  Note  the  graphic  has  been 
truncated  in  the  x  axis  for  viewing. 

4.1.3  COMBINING  MULTIPLE  SENSORS 

Our  goal  is  to  create  a  single  sonar  model  from 
both  the  horizontal  and  vertical  systems.  Our 
methodology  is  to  maintain  separate  occupancy 
grids  and  combine  them  using  an  independent 
opinion  pool.  Assuming  the  sonar  creates 
occupancy  grids  Pi  and  P2.  The  grid  probabilities 
can  be  combined  as  a  pooled  probability: 

PA 

p1/>+(i-JpI)(i-Jp2) 

The  individual  occupancy  grid  values  are 
dependent  only  on  the  sensor  input  and  its 
probabilistic  model.  Maintaining  the 
independence  between  sensors  permits  greater 
flexible  in  adding  or  subtracting  sensors.  For 
example,  if  the  AUV  also  had  a  side-looking 
sonar,  it  could  be  used  as  another  sensor  in  the 
future  for  enhancing  accuracy  of  the  occupancy 
grid.  Compared  with  other  approaches  this  is 
also  computationally  less  intensive  and  more 
appropriate  for  real-time  robotic  applications. 

5.  RESULTS 

The  AUV  was  deployed  on  the  Charles  River 
and  sent  on  a  path  underneath  the  Massachusetts 
Avenue  Harvard  Bridge.  Figure  11  shows  an 
overhead  trajectory  plot  with  the  bridge  pylon 
positions.  GPS  positions  of  the  pylons  were 
acquired  by  placing  a  handheld  GPS  in  close 
proximity  to  the  pylons  under  the  bridge.  The 


multiple  position  plots  reflect  the  inconsistency 
of  the  GPS  readings  due  to  the  bridge  occluding 
satellite  reception. 

Figures  1  and  2  showed  examples  of  the  vertical 
and  horizontal  images  as  the  AUV  approached 
the  bridge.  Because  of  the  obstacles  seen  in  the 
images  the  AUV  would  not  be  able  to  navigate 
successfully  between  the  pylons  and  over  the 
berm  if  it  relied  on  processing  each  image 
separately. 


Figure  11.  AUV  trajectory  with  annotated 
positions  of  bridge  pylons 

Figure  12  shows  the  horizontal  occupancy  grid 
with  the  bridge  pylon  GPS  positions  plotted. 
Gridded  cells  in  black  indicate  a  probability  of 
.98  or  greater  that  an  object  exists.  It  can  be  seen 
that  by  itself  there  is  not  a  clear  path  under  the 
bridge. 
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Figure  12.  Horizontal  occupancy  grid  with 
GPS  fixes  of  bridge  pylons  (in  green) 

Figure  13  shows  the  three-dimensional  map.  By 
combining  together  both  the  vertical  and 
horizontal  occupancy  grids  into  a  single 


representation,  a  viable  path  is  now  available  to 
the  vehicle  to  pass  underneath  the  bridge.  Only 
through  the  combination  of  horizontal  and 
vertical  FLS  and  three-dimensional  occupancy 
grid  does  the  path  emerge. 


Figure  13.  Occupancy  grid  developed  with  the 
vehicle  on  the  southwest  approach 

6.0  SENSORY-BASED  TRAJECTORIES 

As  apparent  in  the  Figure  13,  the  free  space 
available  for  the  vehicle  is  small.  It  is  defined  by 
the  cross  section  resolution  obtained  by  the 
method  described  above.  This  free  space  can  be 
expanded  to  the  true  distance  between  the  pylons 
by  navigating  a  trajectory  over  time  that  “points” 
the  cross  section  of  the  FLS  at  the  boundaries 
defined  by  the  river  surface  and  bottom  and  the 
nearest  starboard  and  port  bridge  pylons.  In  other 
words,  the  environmental  map  is  conservatively 
represented  by  limitations  in  the  field  of  view  of 
the  cross  section  of  the  sonar.  By  ensonifying 
additional  areas  in  front  of  the  vehicle  it 
improves  map  resolution  and  provides  greater 
situational  awareness  for  the  vehicle. 

A  trajectory  that  increases  the  fidelity  of  the  map 
through  “pointing”  the  cross  section  of  the 
forward  looking  sonar  has  two  competing 
objectives;  the  ability  to  maintain  a  straight  line 
path  and  the  ability  to  fully  ensonify  a 
navigational  hazard.  Both  of  these  objectives  can 
be  satisfied  by  a  helix  maneuver.  Helical  motion 
is  obtained  by  using  the  cross-section  beam 
model  of  the  sonar  and  controlling  the  vehicle  in 
such  a  way  as  to  ensonify  in  a  clockwise  manner 
the  starboard  pylon,  the  water  surface,  the  port 
pylon  and  the  riverbed. 

The  control  law  that  represents  this  helix  motion 
can  be  parameterized  in  Cartesian  coordinates  by 


the  following  equations: 

x(t)  —  at 
y{t)=pCOEk) 
z(t)  =  Ssr\[t) 

where  jc,  y,  and  z  represents  longitudinal, 
latitudinal  and  altitude  positions  for  the  AUV 
and  a  is  a  scalar  for  circular  periodicity  of  the 
helix,  /3  is  the  semi-major  axis  radius  for  the 

ellipse  (as  viewed  in  the  y,  z  plane)  and  <5  is 
semi-minor  axis  radius. 

Define  the  bridge  opening  as  \j/0 ,  as  the 
difference  between  the  vehicle  headings  when 
oriented  toward  the  port  and  starboard  pylon. 
Define  the  vertical  opening  as  0o ,  as  the 
difference  between  the  water  surface  and 
riverbed. 

Wo  =  ¥PP  ~  ¥sP 

Qn=d  -0 

O  pp  sp 

The  vertical  and  horizontal  angles  represented 
by  y/0 , 60  increase  as  a  function  of  speed  and 
distance  to  the  bridge.  There  are  two  interesting 
considerations  in  the  development  of  the  helical 
trajectory:  First  the  commands  to  the  rudder  and 
dive  plane  are  90  degrees  out  of  phase.  Second, 
the  amplitude  of  the  commands  are  increasing 
this  reflects  the  increasing  aperture. 

At  some  point  the  vehicle  is  unable  to  maintain 
the  helical  trajectory  while  maintaining  a 
reasonable  forward  heading.  This  can  be 
properly  accounted  for  through  the  control 
parameters  a ,  /?,  and  8  .  Figure  14  shows  an 
example  of  initial  results  from  testing  helical 
trajectories  in  Monterey  Bay,  CA  onboard  the 
NPS  REMUS  AUV. 

While  more  rigorous  analysis  is  required,  in 
general,  the  AUV  must  make  a  decision  to  move 
forward  within  approximately  20-30  meters  of 
the  obstacle. 


Figure  14.  Helical  Trajectory  from  the  NPS 
REMUS  in  Monterey  Bay,  CA 

6.0  FRENET-SERRET  PATH  FOLLOWING 

An  important  consideration  in  the 
implementation  of  the  helical  trajectory  is  the 
ability  of  the  controller  to  follow  the  calculated 
path  over  time.  For  the  secondary  controller  this 
is  accomplished  through  the  kinematic  path¬ 
following  controller  developed  in  [7].  Figure  16 
depicts  the  coordinate  frames  used  to  develop 
this  controller. 


Figure  15.  The  inertial  {1}  frame,  Serret- 
Frenet  {F}  error  frame  and  the  body-fixed 
reference  frame  describing  the  problem 
geometry. 

Frame  {1}  is  the  inertial  reference  frame.  Frame 
{F}  is  a  Serret-Frenet  frame  attached  to  an 
arbitrary  point  on  the  path  described  by  the 
vector  pc  (red).  Frame  |b]  is  the  body-fixed 
reference  frame  aligned  with  the  AUV  forward 
velocity  vector.  From  this  geometry,  it  is 
possible  to  establish  a  relationship  between 
vector  qI?  the  inertial  position  of  the  AUV,  and 
the  vector  qF,  its  position  of  expressed  in  the 
Serret-Frenet  frame  attached  to  the  spatial 
trajectory.  This  produces  the  simple  kinematic 
model: 


Xj-U  COS/COSI// 

yI-U  cos  7  sin  y/ 

Zj  =  -U  sin  7 

if  —  q 

l 

if/  = - r 

cos  7 

where  y  is  the  flight  path  angle  above  the 
horizontal  plane  and  y/  is  the  heading  angle. 

Since  the  spatial  trajectory  is  defined  by  the 
parametric  equations  above,  it  is  a  simple 
exercise  to  compute  the  tangent  (T),  normal  (N), 
and  binormal  (B)  unit  vectors  of  frame  {F}.  In 
addition,  the  angular  velocity  of  frame  {F}  with 
respect  to  the  inertial  frame  {I},  resolved  in  {F} 
depends  only  upon  the  curvature,  K  ,  and  the 
torsion,  of  the  Serret-Frenet  frame.  This 
angular  velocity  is  proportional  to  the  speed  at 
which  a  virtual  AUV  traverses  the  curve,  l, 
where  /  is  the  path  length  of  the  curve.  We  can 
then  combine  the  T,  N,  B  unit  vectors  into  the 
[TNB]  rotation  matrix  from  to  represent  rotation 
from  the  {F}  to  {1}  frame.  Now  that  we  have  the 
necessary  equations  to  relate  qh  qF,  and  pc  and 
how  they  evolve  in  time,  we  can  create  a  state- 
space  representation  of  the  error  kinematics — 
that  is,  the  position  and  attitude  (Euler  angles)  of 
the  AUV  body-fixed  frame  relative  to  the  Serret- 
Frenet  frame,  which  is  the  desired  position  and 
orientation  for  the  vehicle  to  assume  while 
traversing  the  helix.  In  this  representation: 

xF  -  -1(1  -  KyF)  +  U cos 0e  cosi/a, 
yF  =  -1(kxf  -gzF)  +  U cos de  sin i \re 
zF  =  -lgyF  -  u  sin  0e 

9e  =  ue 
We  =  UV 

1  =  Kxxf  +  U  cos  6e  cos  y/e 

In  the  above  kinematics,  I  controls  the  speed  of 
the  virtual  AUV  we  wish  to  “follow,”  while  Ue 
and  U¥  are  control  inputs  for  pitch  rate  and  turn 

rate,  respectively,  typically  suitable  for  sending 
to  a  primary  autopilot.  Reference  [7]  contains  a 
detailed  development  of  the  control  input  signals 


UQ  and  UQ  used  to  reduce  the  error  kinematics 

to  zero  (i.e.  drive  the  AUV  onto  the  desired 
trajectory)  and  provides  a  proof  of  this 
controller’s  exponential  stability.  It  should  be 
stated  that  in  its  present  form,  the  REMUS 
control  protocol  does  not  accept  pitch  rate 
commands.  Therefore,  in  the  preliminary 
experiments  with  this  type  of  trajectory,  the 
horizontal  and  vertical  plane  motions  were  de¬ 
coupled  so  that  higher-level  depth  commands 
could  be  used  to  control  the  vertical  motion. 
Experimental  data  may  provide  a  means  for 
controlling  pitch  rate  by  adjusting  the  profile  of 
these  depth  commands.  Figure  12  depicts  the 
helical  trajectory  with  the  superimposed  tangent, 
normal  and  binormal  unit  vectors  of  the  body 
frame. 


Figure  16.  The  AUV  Helical  Trajectory 

6.0  CONCLUSIONS 

In  this  paper,  we  demonstrated  the  ability  to  use 
complimentary  sonar  models  to  build  a  three 
dimensional  probabilistic  occupancy  grid.  This 
methodology  was  used  to  discover  forward 
trajectories  underneath  a  bridge  that  weren’t 
otherwise  available.  A  critical  aspect  to  building 
the  occupancy  grid  is  a  probability  distribution 
for  empty  and  occupied  grid  cells.  A  histogram 
approach  was  used  for  the  generating  these 
distributions.  Results  were  demonstrated  using  a 
Charles  River  data  set  where  the  AUV  navigated 
underneath  the  bridge  avoiding  the  pylons. 
Finally  a  helical  trajectory  was  introduced  that 
permits  the  vehicle  to  fully  take  advantage  of  the 
cross  section  of  the  vertical  and  horizontal  sonar 
while  maintaining  a  forward  path.  Critical  in 
developing  non-traditional,  non-linear  AUV 


paths  are  ensuring  the  vehicle  can  track  the  path, 
this  is  accomplished  through  the  use  of  the 
kinematic  path  following  controller  utilizing  the 
Frenet-Serret  framework. 
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